/*
  Double DC Motor driver class (Use DCMotor)
  Hitoshi Yamada <ouch@emergeplus.jp>

  History:  26/August/09 - V0.1 released.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include <wiring.h>
#include <DoubleDCMotor.h>

// Initialize motor driver
void DoubleDCMotor::Init(int L_FIN, int R_FIN, int L_RIN, int R_RIN, int L_VREF, int R_VREF, int motorVIN, int boardVIN, float cal)
{
  // Save calibration rate
  calibration = cal;
  // Initialize Left Motor
  L_Motor.Init(L_FIN, L_RIN, L_VREF, motorVIN, boardVIN);
  // Initialize Right Motor
  R_Motor.Init(R_FIN, R_RIN, R_VREF, motorVIN, boardVIN);
}

// Go
void DoubleDCMotor::Go(int speed)
{
  Free(); 
  L_Motor.Forward(speed * calibration);
  R_Motor.Forward(speed);
}

// Go with interval
void DoubleDCMotor::Go(int speed, int interval)
{
  Go(speed);
  delay(interval);
}

// Back
void DoubleDCMotor::Back(int speed)
{
  Free();
  L_Motor.Backward(speed * calibration);
  R_Motor.Backward(speed);
}

// Back with interval
void DoubleDCMotor::Back(int speed, int interval)
{
  Back(speed);
  delay(interval);
}

// Stop
void DoubleDCMotor::Stop(void)
{
  Free();
  L_Motor.Break();
  R_Motor.Break();
}

// Stop with interval
void DoubleDCMotor::Stop(int interval)
{
  Stop();
  delay(interval);
}

void DoubleDCMotor::TurnLeft(int speed)
{
  Free();
  L_Motor.Backward(speed * calibration);
  R_Motor.Forward(speed);
}

void DoubleDCMotor::TurnLeft(int speed, int interval)
{
  TurnLeft(speed);
  delay(interval);
}

void DoubleDCMotor::TurnRight(int speed)
{
  Free();
  L_Motor.Forward(speed * calibration);
  R_Motor.Backward(speed);
}

void DoubleDCMotor::TurnRight(int speed, int interval)
{
  TurnRight(speed);
  delay(interval);
}

void DoubleDCMotor::Free(void)
{
  L_Motor.Free();
  R_Motor.Free();
  delay(1);
}

